/*
 * cmucam.c
 *
 *  Created on: Sep 30, 2014
 *      Author: amorim
 */

#include <cv.h>
#include <highgui.h>
#include "cmucam.h"
#include "util.h"

CMUCam::CMUCam() {
	device = "/dev/ttyAMA0";
}

void CMUCam::connect() {

	fd = connection.openSerialPort(device.c_str());
}

void CMUCam::set_monochrome() {

	char tmp[100];
	connection.tx_message(fd, 3, "GV");
	msleep(50);
	connection.rx_message(fd, tmp, 100);

	connection.tx_message(fd, 5, "CS 3");
	msleep(50);
	connection.rx_message(fd, tmp, 50);
}

void CMUCam::set_rgb() {
	char tmp[100];

	connection.tx_message(fd, 3, "GV");
	msleep(50);
	connection.rx_message(fd, tmp, 100);

	connection.tx_message(fd, 5, "CS 2");
	msleep(50);
	connection.rx_message(fd, tmp, 100);
}

int CMUCam::snapshot(string file) {

	FILE* fp = fopen(file.c_str(), "w");
	char tmp[512];
	int size = 0, total = 0, cnt_delay = 0;
	string cmd = "SJ"; // send JPEG frame

	connection.tx_message(fd, 3, "GV");
	msleep(50);
	size = connection.rx_message(fd, tmp, 512);
	connection.tx_message(fd, 3, cmd.c_str());

	msleep(100);

	size = connection.rx_message(fd, tmp, 512);

//	char nck[10];
//	memcpy(nck, tmp,);

	while (size >= 0 && cnt_delay < 10) {

		if (size > 0) {
			total += size;
			fwrite(tmp, sizeof(char), size, fp);
			//printf(" %d bytes read. TOTAL: %d\n", size, total);
		} else if (size == 0) {
			msleep(200);
			cnt_delay++;
		} else {
			break;
		}
		size = connection.rx_message(fd, tmp, 512);

	}
	fputc(EOF, fp);
	fclose(fp);
	return total;
}

void CMUCam::close() {
	connection.close(fd);
}
